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ABSTRACT 


A  simplified  description  of  robotic  manipulator 
Is  In  terms  of  Its  aero  reference  position.  It  re¬ 
quires  the  specification  of  the  Joint  axes  directions 
and  the  coordinates  of  points  locating  the  joint  axes 
In  the  base  coordinate  system.  This  description  can 
be  learned  quickly  and  Is  not  prone  to  the  errors  of 
Interpretation.  It  has  previously  been  used  to  de¬ 
rive  closed  form  Inverse  kinematic  solutions  for 
simple  manipulators  as  veil  as  to  develop  efficient 
numerical  solutions  for  general  manipulators.  This 
paper  develops  manipulator  dynamics  In  an  extended 
zero  reference  position  description.  The  recursive 
Nevton-Euler  formulations  for  the  problems  of  in¬ 
verse  and  direct  dynamics  are  presented  in  this 


1 .  INTRODUCTION 


The  dynamics  of  robot  arms  has  been  considered 
by  several  Investigators  [1-10].  A  common  formula¬ 
tion  Is  based  upon  the  Denavit  and  Hartenberg  kine¬ 
matic  description  of  spatial  chains  [11-13]  and  the 
use  of  either  4x4  matrices  or  3x3  matrices  along  with 
3x1  column  vectors  for  kinematic  and  dynamic  analyses 
[1-8],  Lagranglan  [1,2,7],  recursive  Lagranglan  [3, 

8],  as  well  as  recursive  Nevton-Euler  [3,4,6]  ap- 
roaches  have  been  used  for  dynamic  formulations. 
Methodologies  leading  to  explicit  computations  of 
actuator  forces  or  torques  have  been  considered  by 
using  dual  matrices  [9]  and  Kane's  dynamical  equa¬ 
tions  [10]. 

The  aforementioned  4x4  matrices  have  twelve  non¬ 
trivial  entries.  In  multiplying  two  4x4  matrices, 
multiplications  with  the  trivial  elements  (zero  and 
one)  can  be  avoided  during  the  programming  stages, 
or  the  concept  of  matrix  partitioning  can  be  used  to 
achieve  some  computational  economy.  Ignoring  such 
possibilities,  references  [5,8]  report  that  the  3x3 
matrix  and  3x1  vector  based  Lagranglan  formulation  Is 
more  than  twice  as  efficient  than  the  4x4  matrix 
based  Lagranglan  formulation.  Among  other  approaches, 
explicit  methods  appear  to  be  the  most  efficient  but 
these  become  dependent  upon  the  configuration  of  the 
ammlpulator.  For  configuration  Independent  formula¬ 
tions,  the  recursive  Nevton-Euler  formulations  ap¬ 
pear  to  be  most  efficient. 

An  alternate  kinematic  description  of  robot 
arms,  called  the  zero  reference  position  description, 
has  been  used  by  Gupta  [14].  It  can  be  mastered 
quickly  and  it  is  not  prone  to  the  errors  of  Inter¬ 
pretation  by  the  user.  Special  cases  such  as  when 
the  adjacent  jolet  axes  become  nearly  parallel  (e.g.  yj 


due  to  manufacturing  errors)  do  not  create  any  prob¬ 
lems  In  this  representation.  The  zero  reference 
position  method  has  been  used  for  cloaed  form  [14,13] 
as  well  as  Iterative  [16]  Inverse  kinematic  robot 
solutions. 

In  this  paper,  which  Is  based  upon  reference 
[17],  we  develop  formulations  for  robot  dynamics  by 
using  an  extended  zero  reference  position  descrip¬ 
tion.  These  formulations  Include:  Inverse  dynamics 
which  Is  the  problem  of  determining  actuator  drive 
forces  or  torques  to  sustain  the  specified  end-effec¬ 
tor  motion  (section  3);  and  direct  dynamics  (or  simu¬ 
lation)  which  Is  the  problem  of  determining  the  end- 
effector  motion  resulting  from  the  application  of 
specified  forces  or  torques  by  the  actuators  (section 
4).  In  view  of  their  relative  efficiency,  only  the 
recursive  Nevton-Euler  formulation  using  the  zero 
reference  position  method  Is  discussed  here;  refer¬ 
ence  [17]  should  be  consulted  for  the  details  of  the 
Lagranglan  formulation. 


2.  ZERO  REFERENCE  POSITION  DESCRIPTION 


In  the  zero  position  description  [14, 15] , a  suit¬ 
able  configuration  of  the  manipulator  Is  designated 
as  the  zero  reference  position  where  all  of  the  Joint 
variables  have  zero  values  (Fig.  1),  In  this  zero  ref¬ 
erence  position  the  unit  vectors  along  the  revolute 
or  prismatic  joints  (u0^  along  the  kth  Joint,  k  ■  1 
to  6)  as  well  as  the  position  vector  of  a  point  on 
the  axis  of  each  Joint  (O^,  k  ■  1  to  6)  are  given 
In  the  base  coordinate  system.  In  Fig.  1,  k  E  1 
for  a  typical  revolute  joint  and  k  E  J  for  a  typical 
prismatic  joint.  In  addition,  the  position  vector 


t  U,° 


Figure  1.  The  zero  reference  posicion 
description  for  typical  revolute  axis  1, 
prismatic  axle  j  and  end-effector. 
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p0h  of  •  reference  point  h  on  the  hand  and  tvo  per¬ 
pendicular  unit  sector*  through  the  point  h  (pref¬ 
erably  an  axial  and  a  transveree  unit  vector,  uoa 
and  uot)  are  also  given.  All  of  the  Joint  variablea 
are  act  to  eero  at  thla  reference  position.  The 
unit  vectors  uok  (k  ■  1  to  6) ,  Uoa  and  uot.  end 
position  vectors  jok  ft  ■  1  to  6)  and  poh  completely 
define  the  kinematic  structure  of  the  manipulator. 

At  a  general  position,  the  governing  kinematic  equa¬ 
tions  of  the  manipulator  can  be  written  as  follows 
(14-17]. 


11  f?(V  V  “ok*  9ok)]  ‘  ‘V 


The  4x4  matrix  [Djj]  represents  the  displacement  of 
the  hand  froa  its  zero  reference  position  to  the 
current  position.  The  current  position  of  the  hand 
is  normally  a  part  of  the  trajectory  specification. 
The  4x4  matrix  (0(9^.  8k.  uDk,  Qok)]  represents  a 
rotation  of  amount  6k  and  a  translation  of  amount  ak 
with  respect  to  the  invariant  line  vector  (u0),.Qok)» 
i.e.  the  vector  uok  passing  through  the  point  Q^. 

In  a  partloned  form,  this  matrix  can  be  written  as 
follows  [13]. 


lP(6k  ■  Bk  ■  “ok  •  Qok>) 

4x4 


?<V  “ok>  ]  *V 


I R ( 6k >  “ok)l  *  (1]  +  [Ursine  +  (l-cos8) 

3x3 


be  established  maelly  [14.16]. 

For  dynamic  formulation,  additional  data  con¬ 
cerning  the  dynamic  properties  of  the  manipulator  is 
also  defined  in  the  Eero  reference  position.  Coin¬ 
cident  points  p£  and  p£+1  are  defined  at  the  center 
of  the  kth  Joint  such  that  p£  Is  the  body  point  of 
the  kth  link  and  p£+1  is  the  body  point  of  the 
(k+l)th  link.  The  following  quantities  are  then 
defined  at  the  Eero  position. 

-ok  body  vector  of  the  kth  Unk  from  pk  to 
the  center  of  mass  k“l 

-ok  body  vector  of  the  kth  link  from  the 
center  of  mass  to  pk  (note  that 
bok  “  £ok  +  ?ok>  k 

Wk  weight  of  the  kth  link 

symmetric  inertia  matrix  of  the  kth  link 
.  about  the  translated  base  coordinate 

'OkJ3x3  system  through  the  mass  center  Gk  when 
the  arm  is  at  zero  positions 

The  vectors  uk.  bk+l •  ck+1  and  dk+i  and  the  time 
varying  inertia  matrix' [l£]  at  the  current  (non-zero) 
configuration  are  computed  as  follows. 

V  ‘  '5k1  {“ok}  «■> 

{bk+l)  •  IV  ^O.k+l1  <2b> 

{ek+i}  ■  '5k1  {e0,k+i}  <2c> 

{'k+l}  “  lV  {d0,k+l}  (2d> 

'h'*  <’*> 


where  [Rk]  is  the  rotation  matrix  of  the  link  k+1 
from  its  zero  position  to  the  current  position.  It 
is  computed  as  follows- 


>V  '?(ei.  “oi*1 


(s  -  !'  (?ok’ 


In  equation  (3),  the  rotation  matrix  [Rfe^,  Uoi)l 
is  the  principal  3x3  minor  of  the  4x4  displacement 
matrix  [ p ( .  si ,  u0j ,  p0i)l  contained  in  equation 
(lb);  it  represents  a  rotation  about  the  axis  u0j. 


If  the  kth  1  oln i  I.  revilute,  then  sk  “  0;  if  the 
kth  Joint  is  prismalii,  then  bg  •  0. 

An  extension  of  the  above  description  [17]  for 
dynamlt  analysis  Is  as  follows.  The  unit  vectors 
uok  (k  •  1  to  6),  uQa  and  uQt  are  known  as  before. 
However,  Instead  of  the  reference  position  vectors 
Qok  for  points  on  the  joint  axes,  reference  body 
vectors  bo,k*l  (k  -  1  to  6)  are  defined  such  that 
bo.k+l  1*  me  oody  vector  of  the  link  k+1  and  it 
connects  the  center  of  the  kth  joint  to  the  center 
of  the  (k+l)th  joint.  The  position  vector  of  the 
joint  center  on  the  kth  axis  in  the  zero  reference 
position  can  be  computed  by  adding  Che  body  vectors 

b<)2 ,  bQ3 . bOk-  fb*  unit  vectors  uok  (k  -  1  to 

6),  uoa  and  Ugt  and  the  body  vectors  b^g  (k  ■  2  to  7) 
completely  define  the  kinematic  structure  of  the 
manipulator.  A  correspondence  among  the  Joint  vari¬ 
ables  using  the  aforementioned  zero  reference  posi¬ 
tion  description  or  the  couzaon  D-H  description  can  VI. 


3.  INVERSE  DYNAMICS  -  ACTUATOR  DRIVE  FORCES 
OR  TORQUES 

The  kth  joint  variable  is  defined  as  qg  such 
that  for  a  revolute  Joint  qg  ■  &k»  end  f°r  •  prismat¬ 
ic  joint  qk  -  sk>  It  is  assumed  that  the  solution 
of  the  problem  of  the  inverse  kinematics  is  available. 
Thus  for  the  specified  rotation  matrix  of  the  hand 
Rh,  position  vector  of  a  reference  point  h  on  the 
hand,  velocity  v>,  of  the  point  h,  and  angular  velo¬ 
city  vector  u'h  or  skew-symmetric  matrix  [fig]  of  Un¬ 
hand,  the  corresponding  Joint  values  0.  (0  •  ( q j  . 
q2,  ....  q&)T)  and  joint  rates  Q  «re  available. 

For  Inverse  kinematic  accelerations  the  follov 
ing  equation,  which  is  used  to  compute  the  Joini 
rates  §,  is  differentiated. 

r  (Jl  <9l  *  |  | 


In  aquation  (4),  [J]  la  the  6x6  velocity  Jacobian 
eta t tlx  of  the  Manipulator  and  Ite  eleaente  are  com¬ 
puted  by  uelng  the  eero-poaltlon  notation.  Equa¬ 
tion  (4a)  la  differentiated  to  obtain  equation  (4b). 

|J]  (Q  )  +  IJ)  {QJ  -  |  jjjj  |  <4b> 


^  tba  ktb  3°lnt  le  revolute 

d.  If  the  kth  Joint  le  prleaatlc  and 
the  collar  le  on  the  kth  link 

?k  +  ^k“k  tb*  ktb  l®*®1  f*  prleaatlc 
end  the  collar  le  on  the 
(k+1 ) th  link  (7) 


where  ah  end  %  are  linear  and  angular  acceleratlone 
of  the' hand  and  [j]  le  a  6x6  matrix  vhoae  eleaente 
are  the  derivatives  of  the  eleaente  of  the  Jacobian 
matrix  (J).  A  recursive  method  to  compute  [J]  Is 
dlacuesed  In  reference  [17].  Equation  (4b)  la  then 
solved  for  the  Joint  accelerations  \5. 

When  the  velocity  and  acceleration  of  the  hand 
as  veil  as  the  corresponding  data  at  the  Joint  level 
(l.e.  Q.  Q,  Q)  are  known,  the  dynamics  of  the  mani¬ 
pulator  can  Be  formulated  as  follows.  A  recursive 
process  for  the  computation  of  the  actuator  forces 
(or  torques)  starts  from  the  7th  link.  At  each 
step,  the  angular  velocity  and  acceleration  of  a 
particular  link,  and  the  linear  acceleration  of  Its 
mass  center  are  computed.  These  values  are  then  used 
to  compute  the  Inertia  force  and  Inertia  moment  act¬ 
ing  on  the  link.  The  Joint  forces  and  torques  are 
then  computed  by  writing  the  dynamical  equilibrium 
equations  (D'Alembert's  principle)  for  that  link. 

In  particular,  when  the  recursive  process  Is 

at  the  kth  link  (k-7,  6,  . . 2),  the  computation 

16  as  follows 


ck  tf  the  (k-l)th  Joint  is  revolute 

_  c.  If  the  (k-l)th  Joint  la  prismatic 

k  -  „  '  and  the  collar  le  on  the  kth  link 

Pk-1  x 

cfc  +  the  (k-l)th  Joint  Is 

prlematlc  and  the  collar  le  on 
the  (k-l)th  link  (B) 

It  should  be  noted  that  the  actuator  forces  (or 
torques)  do  not  depend  on  which  link  carries  the 
collar  of  the  prismatic  Joint.  The  Joint  reactions, 
however,  are  affected  by  this  fact. 

The  acceleration  of  the  mass  center  Gg  le  now 
obtained  from  the  acceleration  of  the  mass  center 
Gfc+l  by  using  the  relation  for  two  points  In  the  same 
body  (equations  9,  11)  and  also  for  coincident  points 
belonging  to  different  bodies  (equation  10).  For 
prismatic  Joints,  note  the  Coriolis  acceleration  term 
in  equation  (10). 


SWi 

st+i 


. {uk  If  the  kth  joint  Is  revolute 

if  the  kth  joint  Is  prismatic 

(5) 


where  Ufc  Is  the  angular  velocity  of  the  kth  link. 
Equation  (5)  is  differentiated  to  compute  the  angu¬ 
lar  acceleration  Ofc. 


k+1 


“  “Ck+1  '  “k+1  X  <SV+|  X  Pk  w  - 


k+1 

?k+l  *  pk  Gk+1 


(9) 


2k+i  -  Vk  '  Vk  *  “k 

if  the  kth  joint  is  revolute 

?k  +  l 

If  the  kth  joint  is  prismatic  (6) 


a  k+j  if  the  kth  joint  la  revolute 
~Pk 

fpk+1  '  Vk  -  2*k  Sir  X  ?k 

If  the  kth  joint  Is  prleaatlc  (10) 


The  next  step  la  the  computation  of  the  acceleration 
of  the  mass  center  of  the  kth  link.  Before  that, 
however,  two  vectors  pk  _C  and  cTpF  (Fig.  2)  are 
defined  as  follows.  k 


‘  !pk  '  “k  *  (Sk  *  CkPk>  '  2k  X  Vk  (11> 


At  this  point  the  linear  acceleration  of  the  mass 
center  Cfc  as  well  as  the  angular  velocity  and  accel¬ 
eration  of  the  kth  link  are  known  and  the  inertia 
force  and  moment  acting  on  this  link  are  computed  In 
the  base  coordinate  system  as  follows  (see  Appendix). 


where  Fk  Is  the  Inertia  force  acting  on  the  kth  link 
and  afc'ls  the  mass  of  the  kth  link. 

V  ■  -ISkH^XSk1  -  <£>  V  «1J>  - 


Figure  2.  IWo  adjacent  links  k  and  k+1. 


where  Nfc  Is  the  inertia  moment  acting  on  the  kth  link,  es 

link,  [Qfc]  le  the  ekew  syaaetrlc  angular  velocity  _ 

matrix ,  and  [ijj]  le  the  time  varying  Inertia  matrix  r 
VI.3vlth  respect  to  the  translated  base  coord,  system  at  C. 
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Figure  3  shows  the  kth  link  with  all  reactive,  grav¬ 
itational,  and  Inertia  forces  and  moments  acting  on 
the  link. 
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Figure  3.  Dynamic  equilibrium  of  link  k. 

Using  the  D'Alembert's  principle,  the  force  and  mo¬ 
ment  equilibria  (dynamic)  equations  for  the  kth  link 
are  written  as  follows 


3k-i 


N. 


“k'!r 


(14) 


Ik-1  •  Ik  -  5k  +  Vl*{  x  Nk  -  p*_lGk  X  (*k  +  ck) 


(15) 


where  Hk_i  and  Tk_j  are  the  reaction  force  and  moment 
exerted  by  the  (k-l)th  link  on  the  kth  link  at  point 
Pk-j.  The  weight  of  the  kth  link  is  Wfc.  The  actuator 
forces  (or  torques)  are  then  computed  as  follows 


fk-l  “  Ik-1  *  IJk-l 


actuator  torque  If  the 
(k-l)th  joint  is  revolute 


fk-i  ■  ?k-i  •  Vi 


actuator  torque  if  the 
(k-l)th  joint  is  prismatic 


(16) 


Equations  (5-16)  constitute  a  recursive  set  of  rela¬ 
tions  for  computing  all  of  the  actuator  forces  or 
torques.  This  formulation  requires  that  body  vectors 
Ufc,  bk,  ck,  dk  as  well  as  the  time  varying  inertia 
matrix  [I{[j  be  computed  at  the  current  position  (eq. 
(2)).  This  process  also  requires  the  computation  of 
the  rotation  matrix  [1^]  in  equation  (3).  Computa¬ 
tion  of  these  quantities  in  equations  (2)  and  (3) 
Involves  a  large  number  of  arithmetic  operations. 

The  algorithm  is  relieved  from  these  excessive  compu¬ 
tations  as  follows.  Let  us  define  *  superscripted 
vectors 


*  *  *  .  k  .  *  *  * 

V  V  *ck'  <W  *  (pk-lV  •  «pk* 

*  *  *  ft 

?k'  V  ?k'  Ik 


by  premultiplying  the  corresponding  vectors 


sv*  5ck'  (ckpk;-  <pk-iV'  5pk-  ?pk_, 

Ik’  !*•  *k»  Ik 


by  tha  3x3  rotation  matrix  CKfc-il1  daflnad  by  aqua¬ 
tion  (3X  Equation  (3)  la  alao  rearranged  as  follows 

I5k.ll*  -  “ok”  'V*  (17) 

In  light  of  tha  abova  definitions,  all  of  tha  equa¬ 
tions  (5-16)  are  premultiplied  by  the  matrix  III  .1 
aa  follows  ‘Sk-U 

#  l?(ek'2ok>  SWi  -  Vok 

>!  «k-j 


eq  (5): 


(IE) 


eq  (6) 


eq  (7):  (GkPk*- 


?(ek*iok>  ^4-1 

*  |  ?(ek'%k>?k+l  -  Vok  -  v£  X  «ok 

:k+i  (>«) 

d  >k 

(20) 


.  l!<ek*!ok>5k4 


i<* 


d  .  +  q,  u  , 
-ok  k-ok 


(8):  (Pk-iV 


-ok 


~ok 


(21) 


iok  T  qk-i“o,k-i 


eq  (9)!  !pfc+i  “  \+-  s£fi  x  <S)k+i  X  <pt+1(W‘> 


2k  X 


(22) 


,  *(ek'?ok)fpk+l 


eq  (10):  a  k  - 


^VSok^J**1  ■  Vok 

2\“u  X  “ok 


(23) 


eq  (11): 


;Gk  “  “pk  -  •Sk  X  (<ek  X  (CkPk)*>  - 


*  k  * 

?k  x  (nkV 


(24) 

(25) 


eq  (12):  Fk  -  ^  a 

k 

eq  (13):  {»£>  -  [1^1  -  [I®k)  (o£)  (26) 


where  [I®kJ  -  {1^1*  “  I5^_i I?k_i  1  lB  the  known 
time  Invariant  inertia  matrix  at  the  xero  reference 
configuration  of  the  ara. 


*<1  U*>.  *k‘,  *  l*<ek-1.5o.k-l>5(>i  -  <  -  V  <27> 

(15):  Tk_*  -  I?(Vl'!?o.k-1>](^-^  + 

x  s  -  x  («: +  £» 


im  titim— i 


VI.  4 


(28) 


•q  06): 


fk-l  "  Ik-1  *  ?0,k-l 

*k-l  "  Cl  •  Vk-l 


(29) 


with  ths  intersecting  X  uli,  while  the  transverse 

wait  'factor  ut  t«ilM  tangent  to  tha  above-mention*.! 
clrcla.  Tharafora  ■'mentioned 


|  *h"“ 


Equations  (18-29)  are  then  used  Instead  of  equations 
(5-16)  for  the  recursive  computation  of  actuator 
forces  (or  torques).  In  modified  equations  the  com¬ 
putation  of  the  body  vectors  and  inertia  matrices  of 
links  at  their  current  position  is  not  required; 
only  the  vectors  and  Inertia  matrices  defined  In  the 
aero  reference  position  are  used. 

The  efficiency  of  the  above  formulation  la 
directly  related  to  the  total  number  of  arithmetic 
(l.e.  multiplications  m,  additions  a)  and  trig¬ 
onometric  (t)  operations.  The  formulation  presented 
in  this  section  hu6  the  following  computational  com¬ 
plexity. 

r-  (136  m  4  118a  +  2  t)  +  P (139  m  +  118  a  4  2  t) 

where  r  is  the  number  of  revolute  joints  and  p  is 
the  number  of  prismatic  Joints.  For  a  6-R  manipula¬ 
tor,  the  computational  complexity  equals  816  m  + 

708  a  +  12  t.  As  a  comparison,  the  Newton-Euler 
formulation  of  the  reference  [4]  requires  851  m 
4  739  a  4  12  t  computations. 

In  a  numerical  example,  the  above  inverse  dyna¬ 
mic  formulation  is  used  to  compute  the  actuator 
torques  which  maintain  a  specified  trajectory  for  a 
6-R  sianlpulator  (Fig.  4).  Tables  1  and  2  contain 
kinematic  and  dynamic  description  of  this  manipula¬ 
tor  in  its  zero  reference  position  shown  In  Fig.  4. 
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Angle  <t>  changes  from  0  to  2n  ,  where  ♦  -  0  indi¬ 
cates  the  top  point  on  the  circle.  For  a  smooth 
start  and  stop  of  the  hand  on  the  trajectory,  the 
l(i(t)  variation  is  selected  according  to  the  follow¬ 
ing  cam  type  scheme  [22-23].  The  total  time  T  for 
the  trajectory  is  divided  into  a  half-cycloidal 
start  (Cl)  of  duration  T|,  a  constant  velocity  seg¬ 
ment  of  duration  T2,  and  a  half-cycloidal  stop  (C2) 
of  duration  T3.  The  transfer  j|i  are  determined  to 
insure  continuity  of  while  V  Is  already  continu¬ 
ous.  This  leads  to  the  start  segment  angle  change 
-  2n  Tj/(T  +  T2) ,  constant  velocity  segment  angle 
change  1P2  “  4n  T2/(T  4  T2)  and  stop  segment  angle 
change  1)13  -  2it  T3/(T  4  T2).  For  Tl_  “_T2  **  T3  »  3 
sec.,  the  following  equations  for  V.  i.  are  ob¬ 
tained  . 
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0  <  t  <  3 
V  n2  .  nt 

*  ’  Ts  *in  T 

’  n  x  lit 

*  "  6  ’  6  cos  T 

,  nt  1  .  nt 

3  <  t  <  6 


•-i 

*  "  I  +  I  (t  -  3) 


6  <  t  <  9 

i/i— jg  sin  j  (9  -  t  ) 
j  tr  ti  tt  , 

*"  6  "  6  608  3  (9  _t) 

*  -  f  4  £  (t  -  6)  4  }  sin  §  (9  -  t  ) 


(32) 


(33) 


(34) 


Then  the  velocity  and  acceleration  specifications  of 
the  hand  are  as  follows 


Figure  4.  The  zero  reference  position  of 
an  industrial  manipulator  (Table  1). 

“h  " 

The  trajectory  of  the  hand  is  specified  as  follows. 

The  point  h  of  the  hand  is  to  move  on  a  circle  of 
radius  6”  in  a  plane  parallel  to  the  base  TZ  plane 
and  its  center  located  on  the  base  X  axis  at  x  -  34". 

The  axial  unit  vector  ua  makes  an  angle  of  £  radians  yj 


(35) 


Tabic  1  Link  and  Joint  information  for  a  PUMA  typa  manipulator  in  ccro-poaltion 


k 

Type  of 
the  k  th 
joint 

U0,k 

C0,k+1 

30,k+l 

^0,k-H 

1 

R 

(0,0,1) 

(0,5,0) 

(0,5,0) 

(0,10,0) 

2 

R 

(0,1,0) 

(8,2,0) 

(9, -2,0) 

(17,0,0) 

3 

R 

(0,1,0) 

(0,0, -9) 

(0,0, -8) 

<0, 0,-17) 

4 

R 

(0,0,1) 

(0.0,-l) 

(0,0,1) 

(0,0,0) 

5 

R 

(0,-l,0) 

(0,-l,0) 

(0,1,0) 

(0,0,0) 

6 

R 

<0,0,-l) 

(0,0. -3) 

(0, 0,-2) 

(0, 0,-5) 

ut 

-  (0,-i,0) 

ua 

-  (0,0,-l) 

Table  2  Mass  and  inertia  Information  for  the  manipulator  of  Table  1  in  tero-position 
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k 

"k 

(lb) 

*Jxx*k 

(lb-in-eec) 

<1yy)k 

<«„>* 

<V* 

(Ix*> 

k  <1ye,k 

2 

10 

.230 

.005 

.230 

0 

0 

0 

3 

16 

.069 

1.453 

1.394 

0 

0 

0 

4 

12 

1.405 

1.585 

.034 

0 

0 

0 

5 

1 

.001 

.001 

.0001 

0 

0 

0 

€ 

1 

.001 

.0001 

.001 

0 

0 

0 

7 

6 

.069 

.069 

.01 

0 

0 

0 

the 

trajectory 

(0  <  t  <  9 

sec) . 

In  Figures  5a  and 

I 

5b, 

the  Joint  variable  q^ 

(i.e. 

64)  makes  a  complete 

-6  * 


0 

sin 


^  +  6  cos  i(i 


-6  if  cos  -  6  ♦  aln  <| i 
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After  solving  the  inverse  kinematics  problem  for 
the  joint  9,  Q,  Q*  the  Joint  actuator  forces  or 
torques  are  computed  by  using  equation  (18-29).  Fig- 


rotation  (2u),  while  the  other  joints  (qj  -  qj, 
qj  -  qg)  return  to  their  starting  values.  The  joint 
variable  q3  (63)  experiences  very  small  changes  using 
the  execution  of  this  particular  trajectory.  The 
computed  drive  torques  for  the  joint  actuators  are 
plotted  in  Figures  8a  and  6b.  Hie  Joint  actuators 
2  and  3  are  most  affected  by  the  gravitational  load¬ 
ing.  The  values  of  the  joint  torques  at  the  begin¬ 
ning  and  the  end  of  the  trajectory  correspond  to 
their  static  equilibrium  values. 

In  the  various  numerical  examples,  it  was  ob¬ 
served  that  the  Inverse  dynamic  computations  of  the 
joint  drive  torques  took  approximately  0.003  CPU 
seconds  per  set  when  these  were  programed  in  double 
precision  Fortran  on  an  IBM  3081. 

The  details  of  the  Inverse  dynamics  in  the  tero 
reference  position  representation  by  using  the  La- 
grangian  formulation  are  presented  in  reference  [17]. 
Although  these  are  too  Involved  to  present  here,  the 


”  w  .V  7 4  8  T;  X  Vx  ,  .development  is  analogous  to  that  in  reference 

(5-7)  show  the  variations  of  Q,  Q,  and  Q  along  VI. o  r 


[8). 


Figure  6(a) 


Figure  6(b) 


Figure  8(a) 


Figure  8(b) 


4.  DIRECT  DYNAMICS  (SIMULATION) 

Direct  dynamics  or  simulation  la  the  problem  of 
determining  the  position,  velocity  and  acceleration 
of  the  hand  when  the  valuea  of  the  Joint  actuator 
forcea  (or  torques)  are  known  as  functions  of  tlae. 

It  la  now  discussed  In  the  context  of  the  tero  ref¬ 
erence  position  description. 

In  general  the  equations  of  motion  for  a  6  D.O.F. 
manipulator  can  be  written  as 

[H(Q)1  {§)  +  {CUtqj.Q))  +  {§(Q>  >  “  <£)  (39) 


•pk  *  !ck  +  *£  x  (s£  *  (ckpk)*)  +  ?k  X  (Vk5‘  <**> 

,  ^VSok*  4 

!pk+l  ‘  t 

?t(qk'-ok^  !pk  +  Vok  +  2qki  x  Hok 


?G.  . .  “  4+1  +  i£+l  *  (£+l  X  <fkCk+/>  + 

k+1  rk 


H(Q)  >6x6  non-singular,  symmetric  inertia 
matrix 

CCq^q’t.  Q)  •  6  x  1  vector  containing  "centri- 

'  '  fugal"  and  "Coriolis"  effects 

g(Q)  >6x1  vector  containing  gravity  and  end- 
effector  loading  effects 

f  -  6  x  1  vector  of  actuator  forces  (or  torques) 

In  the  problem  of  direct  dynamics,  the  Joint 
forces  f  are  known.  Also,  at  the  current  Integration 
step,  the  manipulator  state  variables  Q  and  $  are 
available.  The  linear  system  In  equation  (3§)  is 
solved  for  Q.  The  Joint  accelerations  Q  are  then 
Integrated  numerically  to  compute  the  next  state 
variables  Q  and  q.  The  Newton-Euler  formulation  Is 
utilized  to  define  the  matrix  H  and  vectors  C  and  g 
in  equation  (39).  The  formulation  to  def  lne~  these' 
quantities  Is  similar  to  that  of  the  problem  of  the 
Inverse  dynamics  in  section  3,  except  for  the  follow¬ 
ing  modifications. 

1.  Since  the  position,  velocity  and  accelera¬ 
tion  of  the  hand  are  not  known,  the  recursive  process 
to  compute  the  angular  velocities  and  accelerations 
of  the  links  and  the  linear  accelerations  of  their 
mass  centers  starts  from  the  base  link  (1st  link)  to 
the  hand  (7th  link). 

2.  The  Joint  accelerations  Q  are  not  known. 
Therefore  the  terms  which  are  affected  by  the  Joint 
accelerations  are  defined  as  linear  functions  of  "(J. 
These  terms  for  the  kth  link  are:  a£,  ac-.  M^, 

Tk  and  the  derived  actuator  force  (or  torque)' 
ffc  (29).  In  other  words  these  vectors  in  data  stor¬ 
age  have  the  dimensions  of  3  x  7  (fk  has  the  dimen¬ 
sions  1  x  7).  The  columns  1-6  represent  the  coeffi¬ 
cients  of  q’j  to  ijj  ,  and  the  seventh  column  repre¬ 
sents  the  constant  terms. 

When  the  above  modifications  are  Incorporated 
In  equations  (18-27),  the  equations  (18-29,  22-24) 
are  rearranged  as  follows  to  change  the  recursion. 

'■<w  '£ *  vJ  » 

R  (q  ,u  )  uj  .  If  the  kth  Joint  is 

~°  prismatic  (40) 


5  (V“ok}  [%  +  Vok  +  W  x  “ok1 


?  (V“ok>2k 


°k x 


These  equations  (40-41,  20-21,  42-44)  are  used  for  k 
from  2  to  7  to  compute  u*,  a*  and  ag  for  all  of  the 
links.  Equations  (25-29)  are  then  used  to  compute 
the  six  Joint  actuator  forces  (or  torques)  fk  as 
linear  functions  of  ‘q’^.  The  system  of  linear  equa¬ 
tions  (39)  Is  then  completely  defined  by  using  the 
known  values  of  the  actuator  forces  on  the  right  hand 
side.  On  the  left  hand  side  on  this  equation,  there 
Is  a  6x7  matrix  (the  kth  row  is  fk)  where  the  first 
six  columns  correspond  to  the  matrix  H  and  the  sev¬ 
enth  column  corresponds  to  the  vectors  C  and  g  in 
equation  (39).  To  reduce  the  number  of  arithmetic 
operations  needed  to  define  and  solve  this  aytem 
of  equations,  the  following  observations  were  util¬ 
ized. 

1.  O.  ,  ac-  ,  Fk  and  are  functions  of  q*. 

1  >  1,  2,'. . k-lT  Therefore  the  columns  k,  k+1, 

. . . ,  6  of  the  corresponding  3x7  arrays  are  null  col¬ 
umns.  The  vector  and  scalar  operations  on  these  null 
columns  are  avoided. 

2.  The  inertia  matrix  [H]6jt6  Is  a  symmetric 
matrix  and  therefore  only  the'lower  triangular  part 
of  this  matrix  needs  to  be  computed.  In  actual  Im¬ 
plementation  thlB  means  that  vector  and  scalar  op¬ 
erations  on  columns  k,  k+1,  ...,  6  of  all  3x7  arrays 
In  equations  (25-29)  are  avoided. 

3.  Since  [H]  is  a  symmetric  matrix,  an  effici¬ 
ent  method  such  as  triangular  decomposition  [24]  can 
be  used  to  solve  the  system  of  equations  (39). 

The  formulation  discussed  above  defines  6  second 
order,  ordinary  differential  equations  as  follows 

Q-f(Q,  Q.  t)  (44) 


Simulation  is  basically  the  numerical  solution  of  the 
initial  value  problem  involving  6  second  order  dif¬ 
ferential  equations  (44).  In  terms  of  the  state 
variables  (0,§)  this  system  becomes  a  system  of  12 
first  order'dlff erentlal  equations.  An  efficient 
predictor-corrector  Integration  scheme  is  used  In 
the  computer  program  to  compute  the  state  variables 
Q  and  6  [17]. 

Tne  number  of  operations  required  in  the  simula¬ 
tion  process  prior  to  the  Integration  step  equals 
2468  multiplications,  1879  additions  and  12  trigono¬ 
metric  evaluations.  In  the  numerical  examples  test¬ 
ed,  It  was  observed  that  each  cycle  of  simulation. 
Including  the  integration  step,  took  approximately 
0.018  CPU  on  an  IBM  3081  using  the  double  precision 
Fortran. 

Based  upon  the  zero  reference  position  descrip¬ 
tion  of  robot  arms,  the  inverse  kinematics,  inverse 
dynamics  and  direct  dynamics  (simulation)  have  been 
g Incorporated  into  a  general  purpose  FORTRAN  computer 


program  HASP  -  Manipulator  Analyala  and  Simulation 
Package  which  ie  liated  in  reference  [17]. 

5.  CONCLUSION 

The  Inverse  kinematics  of  manipulators  by  using 
the  zero  reference  position  method  has  been  discussed 
in  references  [14-  16].  In  this  work  the  sero  ref¬ 
erence  position  analysis  method  has  been  extended  to 
formulate  the  problems  of  dynamics  for  general  mani¬ 
pulators.  A  computationally  efficient  formulation 
for  inverse  dynamics  based  upon  recursive  Newton- 
Euler  equations  has  been  developed.  This  formula¬ 
tion  is  then  rearranged  and  modified  to  solve  the 
problem  of  direct  dynamics  (simulation)  for  general 
Industrial  robots. 
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8.  APPENDIX 

Consider  the  generalized  Euler  equations  in  a 
centroidal  body  system  [18  21). 

{?k}b  ■  -^b  nth  {^h  -  L#b  Vb 

Let  [R]  be  the  rotation  matrix  which  relates  the 
coordinates  in  the  body  system  (subscript  b)  to  those 
in  the  translated  base  coordinate  system  (subscript 
tb)  located  at  the  center  of  mass  G. 

{«k}tb  *  {Vb 

■  -  '^’b  nth  H}b  - 1?1  nth  Vb 


10.  Kane,  T.R.  smd  Levinson,  D.A.,  "Use  of  Kane's 
Dynamical  Equations  in  Robotics,"  Int.  J. 

Robotics  Research,  Vol.  2,  No.  3,  1983,  pp.  3- 
20.  VI. 10 


-[Rll^lb  [R1]  •  [R]  [ljlb  I?)1,  [RHu^b 
-  (RHig]b  (RJ1-  (?) 


•Va,  -  -  <£«»«,  <;►>«.  - 

IJk(t^tb  ^?k\b 

or  (Imply,  In  the  translated  base  coordinate  system 

(5fk)  “  ‘  I?k1  !i(t)1  {!Ac}  '  ^k(t)1  {Sk} 

Although  the  forms  of  the  generalized  Euler's  equa¬ 
tions  are  similar  in  the  body  system  and  the  trans¬ 
lated  base  coordinate  system  (18],  the  inertia  ma¬ 
trix  is  time  invariant  in  the  former  while  it  is  a 
function  of  time  in  the  latter. 
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